#include "../include/Debuger.hpp"
namespace ORBSLAM
{
    Debuger::Debuger(/* args */)
    {
    }

    Debuger::~Debuger()
    {
    }

    void Debuger::Show_two_image_match(vector<int> &initial_matches, Frame &initial_frame, Frame &cur_frame)
    {
        vector<DMatch> m;
        m.reserve(initial_matches.size());
        for (int i = 0; i < initial_matches.size(); i++)
        {
            if (initial_matches[i] > 0)
            {
                m.push_back(DMatch(i, initial_matches[i], 5.0));
            }
        }

        cv::Mat img_goodmatch;
        cv::drawMatches(initial_frame.m_image, initial_frame.mv_orb_keypoints, cur_frame.m_image, cur_frame.mv_orb_keypoints, m, img_goodmatch);
        string frame_name = "frame " + to_string(initial_frame.mn_frame_id) + " match frame " + to_string(cur_frame.mn_frame_id);
        cv::imshow(frame_name, img_goodmatch);
        cv::waitKey(0);
        cv::destroyWindow(frame_name);
    }

    void Debuger::Show_two_image_match(KeyFrame *p_KF, Frame &cur_frame, vector<MapPoint *> vp_mpt_match)
    {
        vector<DMatch> m;
        m.reserve(vp_mpt_match.size());
        for (int i = 0; i < vp_mpt_match.size(); i++)
        {
            if (vp_mpt_match[i])
            {
                m.push_back(DMatch(vp_mpt_match[i]->Get_observation()[p_KF], i, 5.0));
            }
        }

        cv::Mat img_goodmatch;
        cv::drawMatches(p_KF->m_image, p_KF->mv_orb_keypoints, cur_frame.m_image, cur_frame.mv_orb_keypoints, m, img_goodmatch);
        string frame_name;
        frame_name = "frame(keyframe) " + to_string(p_KF->mn_frame_id) + " match frame " + to_string(cur_frame.mn_frame_id);
        cv::imshow(frame_name, img_goodmatch);
        cv::waitKey(0);
        cv::destroyWindow(frame_name);
    }

    void Debuger::Show_two_image_match(KeyFrame *p_KF, Frame &cur_frame)
    {
        vector<DMatch> m;
        m.reserve(cur_frame.mvp_mappoints.size());
        int n_match = 0;
        if (0)
        {
            int idx1, idx2;
            for (int i = 0; i < cur_frame.mvp_mappoints.size(); i++)
            {
                if ((cur_frame.mvp_mappoints[i]) && (!cur_frame.mvb_outline[i]))
                {
                    MapPoint *p_mpt = cur_frame.mvp_mappoints[i];

                    if (p_mpt->Get_observation().count(p_KF))
                    {
                        idx1 = p_mpt->Get_observation()[p_KF];
                    }
                    else
                    {
                        continue;
                    }
                    idx2 = i;
                    m.push_back(DMatch(idx1, idx2, 5.0));
                    n_match++;
                }
            }
            cout << "nmatch:" << n_match << endl;
            cv::Mat img_goodmatch;
            cv::drawMatches(p_KF->m_image, p_KF->mv_orb_keypoints, cur_frame.m_image, cur_frame.mv_orb_keypoints, m, img_goodmatch);
            string frame_name;
            frame_name = "frame" + to_string(p_KF->mn_frame_id) + "(keyframe" + to_string(p_KF->mn_keyframe_id) + ") match frame " + to_string(cur_frame.mn_frame_id);
            cv::imshow(frame_name, img_goodmatch);
            cv::waitKey(0);
            cv::destroyWindow(frame_name);
        }
        else
        {
            int idx1,idx2;
            vector<KeyPoint> v_kpt1;
            vector<KeyPoint> v_kpt2;
            v_kpt1.reserve(cur_frame.mvp_mappoints.size());
            v_kpt2.reserve(cur_frame.mvp_mappoints.size());
            for (int i = 0; i < cur_frame.mvp_mappoints.size(); i++)
            {
                if ((cur_frame.mvp_mappoints[i]) && (!cur_frame.mvb_outline[i]))
                {
                    MapPoint *p_mpt = cur_frame.mvp_mappoints[i];

                    if (p_mpt->Get_observation().count(p_KF))
                    {
                        idx1 = p_mpt->Get_observation()[p_KF];
                    }
                    else
                    {
                        continue;
                    }
                    idx2 = i;
                    v_kpt1.push_back(p_KF->mv_orb_keypoints[idx1]);
                    v_kpt2.push_back(cur_frame.mv_orb_keypoints[idx2]);
                    m.push_back(DMatch(n_match, n_match, 5.0));
                    n_match++;
                }
            }
            cout << "nmatch:" << n_match << endl;
            cv::Mat img_goodmatch;
            cv::drawMatches(p_KF->m_image, v_kpt1, cur_frame.m_image, v_kpt2, m, img_goodmatch);
            string frame_name;
            frame_name = "frame" + to_string(p_KF->mn_frame_id) + "(keyframe" + to_string(p_KF->mn_keyframe_id) + ") match frame " + to_string(cur_frame.mn_frame_id);
            cv::imshow(frame_name, img_goodmatch);
            cv::waitKey(0);
            cv::destroyWindow(frame_name);
        }
    }

    void Debuger::Show_two_image_match(Frame &frame1, Frame &frame2, vector<int> &p1, vector<int> &p2)
    {
        vector<DMatch> m;
        m.reserve(p1.size());
        vector<KeyPoint> v_kp1;
        vector<KeyPoint> v_kp2;
        v_kp1.reserve(p1.size());
        v_kp2.reserve(p1.size());
        for (int i = 0; i < p1.size(); i++)
        {

            int idx1 = p1[i];
            int idx2 = p2[i];
            m.push_back(DMatch(i, i, 5.0));
            v_kp1.push_back(frame1.mv_orb_keypoints[idx1]);
            v_kp2.push_back(frame2.mv_orb_keypoints[idx2]);
        }

        cv::Mat img_goodmatch;
        cv::drawMatches(frame1.m_image, v_kp1, frame2.m_image, v_kp2, m, img_goodmatch);
        string frame_name;
        frame_name = to_string(p1.size()) + " point";
        cv::imshow(frame_name, img_goodmatch);
        cv::waitKey(0);
        cv::destroyWindow(frame_name);
    }

    void Debuger::Show_two_image_match(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2, vector<pair<int, int>> &v_match_index)
    {
        vector<DMatch> m;
        m.reserve(v_match_index.size());
        vector<KeyPoint> v_kp1;
        vector<KeyPoint> v_kp2;
        v_kp1.reserve(v_match_index.size());
        v_kp2.reserve(v_match_index.size());
        for (int i = 0; i < v_match_index.size(); i++)
        {

            int idx1 = v_match_index[i].first;
            int idx2 = v_match_index[i].second;
            m.push_back(DMatch(i, i, 5.0));
            v_kp1.push_back(p_keyframe1->mv_orb_keypoints[idx1]);
            v_kp2.push_back(p_keyframe2->mv_orb_keypoints[idx2]);
        }

        cv::Mat img_goodmatch;
        cv::drawMatches(p_keyframe1->m_image, v_kp1, p_keyframe2->m_image, v_kp2, m, img_goodmatch);
        string frame_name;
        frame_name = to_string(v_match_index.size()) + " point";
        cv::imshow(frame_name, img_goodmatch);
        cv::waitKey(0);
        cv::destroyWindow(frame_name);
    }
    
    void  Debuger::Show_two_image(KeyFrame* p_keyframe1,KeyFrame* p_keyframe2)
    {
        int w = p_keyframe1->m_image.cols;
        int h = p_keyframe1->m_image.rows;
        cv::Mat image(h,2*w,p_keyframe1->m_image.type());
        p_keyframe1->m_image.copyTo(image.rowRange(0,h).colRange(0,w));
        p_keyframe2->m_image.copyTo(image.rowRange(0,h).colRange(w,2*w));
        cv::imshow("two selected image",image);
        cv::waitKey(0);
        cv::destroyWindow("two selected image");
    }
} // namespace ORBSLAM
